perm filename TCALC.SAI[HAL,HE]1 blob
sn#199577 filedate 1976-01-30 generic text, type C, neo UTF8
COMMENT ⊗ VALID 00022 PAGES
C REC PAGE DESCRIPTION
C00001 00001
C00003 00002 IFCR ¬DECLARATION(EXTENDED_COMPILATION)
C00004 00003 ! Load module requirements, declarations
C00007 00004 ! SPLANVAL, VPLANVAL, PPLANVAL
C00011 00005 ! Record array handler: NEWARY
C00013 00006 ! Small utilities: PLACESOL, DEVBITS
C00019 00007 ! TRJCLC
C00030 00008 ! Segment time calculators: DEPTIME, RUNTIME
C00036 00009 ! Matrix solvers: DECOMPOSE, SOLVE
C00043 00010 ! POLY, the polynomial spliner: The A matrix
C00053 00011 ! POLY continued: The B vectors
C00058 00012 ! Main body of TRJCLC starts here
C00062 00013 ! Initialize the first node of the motion
C00067 00014 ! Put intermediate points into the thread
C00073 00015 ! Treat the approach
C00080 00016 ! Check for overall time constraints. Fulfil them if possible
C00085 00017 ! Call the polynomial generator on chunks of the motion.
C00093 00018 ! Compute the gravity and inertia terms
C00095 00019 ! Output the motion table
C00107 00020 ! Reclaim all the arrays in the motion thread
C00109 00021 ! CENTCLC, STOPCLC
C00113 00022 ! Bugs
C00115 ENDMK
C⊗;
IFCR ¬DECLARATION(EXTENDED_COMPILATION)
THENC
DEFINE EXTENDED_COMPILATION = "TRUE";
ENTRY;
BEGIN "tcalc"
COMMENT: Source file requirements;
REQUIRE "ABBREV.SAI[S,RHT]" SOURCE_FILE;
REQUIRE "RECAUX.HDR[S,RHT]" SOURCE_FILE;
REQUIRE "HALREC.SAI[HAL,HE]" SOURCE_FILE ;
REQUIRE "EMITER.HDR[HAL,HE]" SOURCE_FILE;
REQUIRE "INTDEF.SAI[HAL,HE]" SOURCE_FILE;
DEFINE $$PRGID "[]" = ["TCALC"];
ENDC;
! Load module requirements, declarations;
REQUIRE "ARMSOL.REL[H,RF]" LOAD_MODULE;
EXTERNAL INTEGER LOSTOP, HISTOP, TIMFAC;
! First word of array [1:14]. LOSTOP and HISTOP are joint limits,
and TIMFAC is the time in jiffies needed to move one degree (or
inch);
EXTERNAL INTEGER PROCEDURE ARMSOL(REFERENCE REAL FIRST_ANG;
INTEGER ARMNUMBER; REAL ARRAY ROT; REFERENCE REAL FIRST_LOC);
EXTERNAL INTEGER PROCEDURE HANDSOL(REFERENCE REAL RES;
INTEGER JOINTNUMBER; REAL ARG);
REQUIRE "FAITRG.REL[1,BES]" LOAD_MODULE;
DEFINE DEBUG = "FALSE";
DEFINE YARM_MECH = "'1";
DEFINE YHAND_MECH = "'2";
DEFINE BARM_MECH = "'4";
DEFINE BHAND_MECH = "'10";
DEFINE AHAND_MECH = "'12";
DEFINE ANARM_MECH = "'5";
DEFINE YARMSB = "'176000";
DEFINE YHANDSB = "'1000";
DEFINE BARMSB = "'770";
DEFINE BHANDSB = "'4";
! SPLANVAL, VPLANVAL, PPLANVAL;
EXTERNAL RANY PROCEDURE GETVALUE(RANY ARG1; RANY ITEMVAR ARG2);
! GETVALUE is to be found in EXPRS.SAI[HAL,RHT];
RPTR(SVAL) PROCEDURE SPLANVAL(RANY OFTHIS; ITEMVAR WORLD);
BEGIN "splanval"
! Returns a scalar as the planning value of this expression;
IF RECTYPE(OFTHIS)=LOC(SVAL)
THEN RETURN(OFTHIS)
ELSE IF RECTYPE(OFTHIS)=LOC(VARIABLE)
THEN BEGIN ! Cheat or use graph structure;
IF VARIABLE:VAL[OFTHIS]
THEN RETURN(VARIABLE:VAL[OFTHIS])
ELSE RETURN(SPLANVAL(GETVALUE(OFTHIS,WORLD), WORLD));
END
ELSE IF RECTYPE(OFTHIS)=LOC(DEXPR)
THEN RETURN(DEXPR:VAL[OFTHIS])
ELSE COMERR("SPLANVAL garbage",OFTHIS);
END "splanval";
RPTR(V3ECT) PROCEDURE VPLANVAL(RANY OFTHIS; ITEMVAR WORLD);
BEGIN "vplanval"
! Returns a vector as the planning value of this expression;
IF RECTYPE(OFTHIS)=LOC(V3ECT)
THEN RETURN(OFTHIS)
ELSE IF RECTYPE(OFTHIS)=LOC(VARIABLE)
THEN BEGIN ! Cheat or use graph structure;
IF VARIABLE:VAL[OFTHIS]
THEN RETURN(VARIABLE:VAL[OFTHIS])
ELSE RETURN(VPLANVAL(GETVALUE(OFTHIS,WORLD), WORLD));
END
ELSE IF RECTYPE(OFTHIS)=LOC(DEXPR)
THEN RETURN(DEXPR:VAL[OFTHIS])
ELSE COMERR("VPLANVAL garbage",OFTHIS);
END "vplanval";
RPTR(VALU$) PROCEDURE PPLANVAL
(RANY OFTHIS; ITEMVAR WORLD; REFERENCE BOOLEAN SUCCESS);
BEGIN "pplanval"
! Returns a sval or frame as the planning value of this place expression;
SUCCESS ← TRUE;
IF RECTYPE(OFTHIS)=LOC(SVAL)
THEN RETURN(OFTHIS)
ELSE IF RECTYPE(OFTHIS)=LOC(FRAME)
THEN RETURN(OFTHIS)
ELSE IF RECTYPE(OFTHIS)=LOC(TRANS)
THEN RETURN(OFTHIS)
ELSE IF RECTYPE(OFTHIS)=LOC(VARIABLE)
THEN BEGIN ! Cheat or use graph structure;
IF VARIABLE:VAL[OFTHIS]
THEN RETURN(VARIABLE:VAL[OFTHIS])
ELSE RETURN(PPLANVAL(GETVALUE(OFTHIS,WORLD), WORLD, SUCCESS));
END
ELSE IF RECTYPE(OFTHIS)=LOC(DEXPR)
THEN RETURN(DEXPR:VAL[OFTHIS])
ELSE SUCCESS ← FALSE;
RETURN(BPARK); ! The default, to prevent more error messages;
END "pplanval";
! Record array handler: NEWARY;
! Arrays in records are reclaimed when record space garbage
collection occurs. This only happens upon explicit call. Therefore,
we use other routines to reclaim arrays.
;
INTERNAL INTEGER PROCEDURE NEWARY(INTEGER LO, HI, DIMS);
BEGIN "newary"
! Returns a new array. DIMS is either 1 or 2, for the number of
dimensions. If it is 2, then the bounds are [LO:HI,0:5];
INTEGER AA;
IF DIMS=1
THEN BEGIN "need1"
REAL ARRAY A[LO:HI];
AA ← MEMLOC(A,INTEGER);
MEMLOC(A,INTEGER) ← 0; ! Defeats deallocation;
RETURN(AA);
END "need1"
ELSE BEGIN "need2"
REAL ARRAY A[LO:HI,0:5];
AA ← MEMLOC(A,INTEGER);
MEMLOC(A,INTEGER) ← 0; ! Defeats deallocation;
RETURN(AA);
END "need2";
END "newary";
! Small utilities: PLACESOL, DEVBITS;
PROCEDURE PLACESOL
(REAL ARRAY RES; RPTR(VALU$) PTR; INTEGER MECH; REFERENCE INTEGER FLG);
! PTR points to a frame or a scalar constant. If it is a frame,
its solution is calculated in RES, if necessary. Otherwise, the
TINFO field is used to store into RES. At the end, both the
TINFO field and RES agree. If armsol has trouble with the
location, then FLG is set FALSE. On the other hand, if it is a
scalar, HANDSOL is called to check bounds (setting FLG) and
to store the result into RES. In any case, MECH specifies
which mechanism (eg BARM) is meant.
;
BEGIN "plcslv"
INTEGER LOJOINT;
LOJOINT ← ARRINFO(RES,1);
FLG ← TRUE;
IF RECTYPE(PTR) = LOC(SVAL)
THEN BEGIN "scaslv"
FLG ← HANDSOL(RES[LOJOINT],LOJOINT,SVAL:VAL[PTR]);
IF ¬(MECH LAND AHAND_MECH)
THEN COMERR("PLACESOL: bad hand specification");
END "scaslv"
ELSE IF RECTYPE(PTR) = LOC(FRAME)
THEN BEGIN "frmslv"
RPTR(TINFO) Q;
IF FRAME:TINFO[PTR]=RNULL THEN
BEGIN "notinfo" ! There is no TINFO field for this frame;
Q ← FRAME:TINFO[PTR] ← NEW_RECORD(TINFO);
MEMLOC(TINFO:ANGLES[Q],INTEGER) ← NEWARY(1,14,1);
END "notinfo"
ELSE Q ← FRAME:TINFO[PTR];
IF TINFO:ANGLES[Q][LOJOINT] = 0
THEN BEGIN "noangs" ! The TINFO has no info about this mechanism;
IF MECH = YARM_MECH
THEN FLG ← ARMSOL(RES[1],0,ROTN:RMX[TRANS:R[FRAME:VAL[PTR]]],
V3ECT:X[TRANS:P[FRAME:VAL[PTR]]])
ELSE IF MECH = BARM_MECH
THEN FLG ← ARMSOL(RES[8],1,ROTN:RMX[TRANS:R[FRAME:VAL[PTR]]],
V3ECT:X[TRANS:P[FRAME:VAL[PTR]]])
ELSE COMERR("PLACESOL: Bad arm specification");
ARRBLT(TINFO:ANGLES[Q][LOJOINT],RES[LOJOINT],6);
END "noangs"
ELSE ARRBLT(RES[LOJOINT],TINFO:ANGLES[Q][LOJOINT],6);
END "frmslv"
ELSE IF RECTYPE(PTR) = LOC(TRANS)
THEN BEGIN "trnslv"
IF MECH = YARM_MECH
THEN FLG ← ARMSOL(RES[1],0,ROTN:RMX[TRANS:R[PTR]],
V3ECT:X[TRANS:P[PTR]])
ELSE IF MECH = BARM_MECH
THEN FLG ← ARMSOL(RES[8],1,ROTN:RMX[TRANS:R[PTR]],
V3ECT:X[TRANS:P[PTR]])
ELSE COMERR("PLACESOL: Bad arm specification");
END "trnslv"
ELSE COMERR("PLACESOL garbage",PTR);
END "plcslv";
PROCEDURE DEVBITS (REFERENCE INTEGER ARM, SBITS, LOJOINT, HIJOINT; RVAR WHAT);
BEGIN "devbits";
IF WHAT = YARM
THEN BEGIN ! Yellow arm;
LOJOINT ← 1;
HIJOINT ← 6;
ARM ← YARM_MECH;
SBITS ← YARMSB;
END
ELSE IF WHAT = YHAND
THEN BEGIN ! Yellow hand;
LOJOINT ← 7;
HIJOINT ← 7;
ARM ← YHAND_MECH;
SBITS ← YHANDSB;
END
ELSE IF WHAT = BARM
THEN BEGIN ! Blue arm;
LOJOINT ← 8;
HIJOINT ← 13;
ARM ← BARM_MECH;
SBITS ← BARMSB;
END
ELSE IF WHAT = BHAND
THEN BEGIN ! Blue hand;
LOJOINT ← 14;
HIJOINT ← 14;
ARM ← BHAND_MECH;
SBITS ← BHANDSB;
END
ELSE BEGIN ! Wrong arm;
COMERR("DEVBITS: No such arm; assuming BLUE.");
LOJOINT ← 8;
HIJOINT ← 13;
ARM ← BARM_MECH;
SBITS ← BARMSB;
END;
END "devbits";
! TRJCLC;
INTERNAL PROCEDURE TRJCLC (RPTR(MOVE$) MOV; ITEMVAR WORLD);
BEGIN "trjclc"
RCLASS THREAD (
REAL STIME, UTIME; INTEGER MODE;
RPTR(STMNT) EVENT;
RPTR(VARIABLE,VALU$) PLACE;
REAL ARRAY ANGLES, VELS; ! [LOJOINT:HIJOINT];
REAL ARRAY COEFF; ! [1:6,0:5]=[joint,degree] polynomial coefficients;
REAL ARRAY GRAVIN; ! [1:12] gravity, inertia terms for each joint;
RPTR(THREAD) NEXT
);
DEFINE TIME_MODE = '3;
DEFINE DEPA_MODE = '4;
DEFINE APPR_MODE = '10;
DEFINE ENDP_MODE = '20;
DEFINE INVI_MODE = '40;
!
Data structures:
A THREAD is a linked list of points along which the trajectory
passes. It has these fields:
MODE(INTEGER)
The TIME_MODE bits relate to UTIME:
0:no bound, 1:lower bound, 2:upper bound, 3:exact bound.
DEPA_MODE: on if this point is a departure
APPR_MODE: on if this point is an approach
ENDP_MODE: on if this point is an endpoint (either one)
INVI_MODE: on if this point ends a segment whose time is
inviolate. This applies to the endpoint segments only.
STIME(REAL)
System-calculated time in seconds since previous node. If
there is ia conflict between user and system, then the
resolved time is placed in STIME. That causes problems: The
system time is destroyed, so global resolutions use whatever
foolish thing the user wanted.
UTIME(REAL)
User-supplied time in seconds since previous node.
PLACE(RVAR)
Variable (eventually expression) which has the location of
the mechanism as this node is achieved. This can refer to an
arm (in which case the variable will be a frame) or a hand
(in which case it will be a scalar).
ANGLES(n-VECTOR)
Joint angles for this node, if there is an associated place.
VELS(6-VECTOR)
Joint velocities for this node (deg/sec) if there are some.
EVENT(PTR(STMNT))
Code to start up when this node reached.
COEFF(6x6 MATRIX)
The 6 coefficients of the segment ending at this node.
NEXT(PTR(THREAD))
Next node.
The trajectory calculator turns motion specifications into
interpretable tables. At the moment it allows any one mechanism,
that is, one arm or one hand. Future work will allow any
combination of mechanisms. The tables are calculated by the
following method:
A thread is made, with a node for each place in the motion
specification, that is, the initial point, the departure, if any,
the via points, the approach point, and the destination. Arm or
hand solutions are calculated for each node. It may be that this
serial calculation will lead to flips of the arm. If this
happens, the proper order is outside-in. This is because the
ARMSOL routine uses the previous solution to resolve ambiguities
in joint 4 of the Scheinman arms.
Any deproach points or calculated via points or calculated
destinations must have code emitted to make a cell for them in
the graph structure. The cell for a departure is marked
permanently invalid. Its calculator uses the hand position
itself, not the place where the arm was to be at the start of the
motion. The cells for the calculated via points and the approach
point are in the graph structure in the usual way. This code
must be emmitted at the outermost practical point in the program:
If it is too far in, then it gets redone too often, and if it is
too far out, it might cause graph structure to hang around
associated to non-existent nodes. In any case, it is necessary
to put such code at a block entry, and to be sure to get rid of
the resulting graph structure at block exit. The current code
does not handle any of this.
At this time, the fourth degree polynomials for deproach segments
are calculated, and any given velocity constraints are noted.
The presence of a velocity constraint implies that the
acceleration is constrained to zero. If the user has supplied a
time, it is put in UTIME, and STIME is computed by the system.
If they are compatible, STIME is modified to the final decision
on the time for the segment.
After the entire thread is made, a global check is made to
insure that the timing is in agreement with the user's wishes.
Then the thread is divided into chunks, where each chunk is the
region between two velocity-constrained points (the deproach
points are such). A chunk which has only two points (but not a
deproach chunk, for which the trajectory has already been
calculated) gets a fifth-degree polynomial calculated to match
all the constraints. A chunk with more points requires splining
for the trajectory. The first step is to insert one
unconstrained point in each of the two longest intervals. It has
been found that the best place for these points is almost at one
end of the intervals (.001 of the way to the end) to
minimise overshoot problems. After the fully unconstrained nodes
have been inserted into the thread, the routine POLY is called to
create the coefficients of the third degree splined polynomial.
It has been found that using fourth degree polynomials in two of
the segments instead of inserting two unconstrained points leads
to uncontrollable overshoot. Finally, the resulting trajectory
is emitted.
The following conventions are used for arms and joints. Joints
1-6 are yellow arm (arm 0), and joint 7 is the yellow fingers
(arm 2). Joints 8-13 are the blue arm (arm 1), and joint 14 is
the blue fingers (arm 3). The angle arrays are tailored to have
whatever joints are needed. The arm and hand solution programs
are told which mechanism to expect.
The current code does not check location, velocity or
acceleration bounds except for location bounds at user-specified
places. Instead, location bounds are to a large extent insured
by the servo. Velocity and acceleration can be optimized by
rescaling time, in the cases when the user has not specified any
time in the entire motion, nor any velocities, but this is not
currently attempted.
;
! Segment time calculators: DEPTIME, RUNTIME;
INTERNAL BOOLEAN CAUTIOUS; ! If TRUE, motions are slower;
REAL PROCEDURE DEPTIME(REAL ARRAY ENDANG, DEPANG, DEL);
BEGIN "deptim"
! Uses the endpoint and deproach point joint angles and
determines the correct time for the deproach between them,
which it returns. If the deproach point is more than half
the way from the endpoint to the joint limit, it is moved to
the halfway point. This means that DEPANG CAN BE CHANGED BY
THIS PROCEDURE!! This is to prevent deproaches which
overstrain the arm. The resulting DEPANG-ENDANG is stored in
DEL.
;
INTEGER JOINT, ! For loop control;
LOJOINT, HIJOINT; ! Defines which arm;
REAL DELTA, ! For joint angle differences;
TAU, TTAU, ! For times;
TEMP;
LOJOINT ← ARRINFO(ENDANG,1);
HIJOINT ← ARRINFO(ENDANG,2);
! If the distance from ENDANG to DEPANG is more than half
the distance to the joint limit, then modify DEPANG to fall
within such a restriction;
FOR JOINT ← LOJOINT STEP 1 UNTIL HIJOINT DO
BEGIN ! Each iteration checks one joint for stop violation;
DEL[JOINT] ← DELTA ← DEPANG[JOINT] - ENDANG[JOINT];
IF (TEMP ← MEMORY[LOC(HISTOP)-1+JOINT,REAL]-ENDANG[JOINT]) < 2*DELTA
THEN BEGIN ! Exceed high stop;
DEPANG[JOINT] ← (MEMORY[LOC(HISTOP)-1+JOINT,REAL]+ENDANG[JOINT])/2.;
DEL[JOINT] ← TEMP / 2.;
END
ELSE IF
(TEMP ← MEMORY[LOC(LOSTOP)-1+JOINT,REAL]-ENDANG[JOINT]) > 2*DELTA
THEN BEGIN ! Exceed low stop;
DEPANG[JOINT] ← (MEMORY[LOC(LOSTOP)-1+JOINT,REAL]+ENDANG[JOINT])/2.;
DEL[JOINT] ← TEMP / 2.;
END;
END;
TAU ← TTAU ← 0.;
! Find maximum required time for the motion;
FOR JOINT ← LOJOINT STEP 1 UNTIL HIJOINT DO
BEGIN
TTAU ← MEMORY[LOC(TIMFAC)-1+JOINT,REAL] * ABS(DEL[JOINT]);
IF TTAU ≥ TAU THEN TAU ← TTAU;
END;
! If you want to use LOU's model of linear accleration,
you should insert here:
IF (TEMP←.4*SQRT(distance moved in cm.)) > TAU THEN TAU ← TEMP;
! This .4 (sec*sqrt(cm)) is based on experience (LOU
1/13/75), it accounts for the distance moved according
to a linear acceleration model;
IF CAUTIOUS THEN TAU ← TAU * 2;
RETURN (TAU/60. + .3); ! Add on some slack time,
convert jiffies → seconds;
END "deptim";
REAL PROCEDURE RUNTIME(REAL ARRAY OLDANG, NEWANG);
BEGIN "runtim"
! Uses the old and the new joint angles to determine the
correct time in seconds for one segment of motion;
INTEGER JOINT, ! For loop control;
LOJOINT, HIJOINT; ! Defines which arm;
REAL TAU, TTAU;
TAU ← TTAU ← 0;
LOJOINT ← ARRINFO(OLDANG,1);
HIJOINT ← ARRINFO(OLDANG,2);
FOR JOINT ← LOJOINT STEP 1 UNTIL HIJOINT DO
BEGIN
TTAU ← MEMORY[LOC(TIMFAC)-1+JOINT,REAL] *
ABS(OLDANG[JOINT]-NEWANG[JOINT]);
IF TTAU ≥ TAU THEN TAU ← TTAU;
END;
IF CAUTIOUS THEN TAU ← TAU * 2;
RETURN (TAU/60. + .3); ! Add on some slack time,
convert jiffies → seconds;
END "runtim";
! Matrix solvers: DECOMPOSE, SOLVE;
SAFE OWN INTEGER ARRAY PS[1:50];
SIMPLE PROCEDURE DECOMPOSE(INTEGER N;SAFE REAL ARRAY A,LU);
! Both A and LU are [1:N, 1:N]. Uses global array PS.
Computes triangular matrices L and U and permutation matrix
PS so that LU=PA. Stores (L-I) and U both in LU. The call
DECOMPOSE(N,A,A) will overwrite A with LU.
;
BEGIN "decompose"
INTEGER I, J, K, PIVOTINDEX;
REAL NORMROW, PIVOT, SIZE, BIGGEST, MULT;
SAFE OWN REAL ARRAY R[1:50];
SIMPLE PROCEDURE ILOOP(INTEGER UL;REFERENCE REAL R1,R2);
! Machine-coded for efficiency;
START_CODE
LABEL LP,EU;
MOVE 1,-1('17);
MOVE 2,-2('17);
MOVE 3,-3('17);
SUB 3,K;
JUMPLE 3,EU;
LP: AOJ 1,;
AOJ 2,;
MOVN 4,MULT;
FMPR 4,(1);
FADRM 4,(2);
SOJG 3,LP;
EU: END;
IF N > 50
THEN COMERR("DECOMPOSE can't handle a matrix as large as" & CVS(N),RNULL);
! Initialize PS,LU and R;
FOR I←1 STEP 1 UNTIL N DO
BEGIN
PS[I]←I;
NORMROW←0;
FOR J←1 STEP 1 UNTIL N DO
BEGIN
LU[I,J]←A[I,J];
IF (NORMROW<ABS(LU[I,J])) THEN NORMROW←ABS(LU[I,J]);
END;
IF (NORMROW≠0)
THEN R[I]←1/NORMROW
ELSE BEGIN
R[I]←0;
COMERR("Zero row in DECOMPOSE",RNULL);
END;
END;
! Gaussian elimination with partial pivoting;
FOR K←1 STEP 1 UNTIL N-1 DO
BEGIN "kloop";
BIGGEST ← 0;
FOR I ← K STEP 1 UNTIL N DO
BEGIN
SIZE←ABS(LU[PS[I],K])*R[PS[I]];
IF (BIGGEST<SIZE)
THEN BEGIN
BIGGEST←SIZE;
PIVOTINDEX←I;
END;
END;
IF BIGGEST = 0
THEN BEGIN
COMERR("Singular matrix in DECOMPOSE",RNULL);
DONE "kloop";
END;
IF PIVOTINDEX ≠ K
THEN BEGIN
J←PS[K];
PS[K]←PS[PIVOTINDEX];
PS[PIVOTINDEX]←J;
END;
PIVOT←LU[PS[K],K];
FOR I←K+1 STEP 1 UNTIL N DO
BEGIN
LU[PS[I],K]←MULT←(LU[PS[I],K]/PIVOT);
IF MULT ≠ 0
THEN ILOOP(N,LU[PS[I],K],LU[PS[K],K]);
! The following is the result of the machine code:
FOR J ← K+1 STEP 1 UNTIL N DO
LU[PS[I],J]←LU[PS[I],J]-MULT*LU[PS[K],J];
END;
END "kloop";
IF (LU[PS[N],N]=0)
THEN COMERR("Singular matrix in DECOMPOSE",RNULL);
END "decompose";
SIMPLE PROCEDURE SOLVE(INTEGER N;SAFE REAL ARRAY LU,B,X);
! Arrays LU[1:N,1:N], B,X[1:N]. Uses global safe integer
array PS. Solves AX=B using LU from DECOMPOSE.
;
BEGIN "solve"
INTEGER I,J;
REAL DOT;
SIMPLE PROCEDURE ILOOP(INTEGER LL,UL;REFERENCE REAL R1,R2);
! Machine-coded for efficiency;
START_CODE
LABEL LP,EU;
MOVE 1,-1('17);
MOVE 2,-2('17);
MOVE 3,-3('17);
SUB 3,-4('17);
SETZ 4,;
JUMPL 3,EU;
LP: MOVE 5,(1);
FMPR 5,(2);
FADR 4,5;
AOJ 1,;
AOJ 2,;
SOJGE 3,LP;
EU: MOVEM 4,DOT;
END;
FOR I ← 1 STEP 1 UNTIL N DO
BEGIN
ILOOP(1,I-1,LU[PS[I],1],X[1]);
! Has this effect:
DOT←0
FOR J←1 STEP 1 UNTIL I-1 DO
DOT←DOT+LU[PS[I],J]*X[J];
X[I]←B[PS[I]]-DOT;
END;
X[N] ← X[N] / LU[PS[N],N];
FOR I ← N-1 STEP -1 UNTIL 1 DO
BEGIN ! RF: I changed loop upper index from N, to avoid
subscript errors;
ILOOP(I+1,N,LU[PS[I],I+1],X[I+1]);
! Has this effect:
DOT←0
FOR J←I+1 STEP 1 UNTIL N DO
DOT←DOT+LU[PS[I],J]*X[J];
X[I]←(X[I]-DOT)/LU[PS[I],I];
END;
END "solve";
! POLY, the polynomial spliner: The A matrix;
PROCEDURE POLY (RPTR(THREAD) FIRST, LAST; INTEGER LOJ, HIJ, NS);
! Calculate a trajectory for joints LOJ through HIJ using
the thread from FIRST to LAST. The number of segments in the
chunk is given by NS. The location for each node is to be
found in THREAD:ANGLES[*][JOINT], except for the
unconstrained points, which are distinguishable in that
THREAD:PLACE[*] = RNULL. The velocities of the first and
last points are given in THREAD:VELS[*][JOINT]. It is assumed
that the accelerations at these points are to be zero. The
timing for each segment is found in THREAD:STIME[*] in the
node at the end of the segment. The coefficients of the
resulting polynomial will be stored in the thread nodes, as
THREAD:COEFFS[*][JOINT,degree]. ;
BEGIN "poly"
DEFINE MEM(ARG) "<>" = <MEMORY[ARG,REAL]>;
SAFE REAL ARRAY A [1:4*NS,1:4*NS];
SAFE REAL ARRAY B, X [1:4*NS];
! A is a large matrix and B is a vector, for which we will
solve AX=B. A is the same for each joint, but B is
calculated anew for each joint. Thus only one call to
DECOMPOSE is needed;
RPTR(THREAD) P, Q; ! Used in tracking down the motion thread;
INTEGER ROW, COL, N, SEG, ALOC, I, JOINT; ! ALOC is used to point into A;
REAL TEMP;
ARRCLR(A);
N ← 4 * NS;
ROW ← COL ← 1;
! Compute the A matrix decomposition:;
! Fill the A matrix for the first segment;
ALOC ← LOCATION(A[1,1]);
A[ROW,COL] ← A[ROW+1,COL+1] ← 1.;
A[ROW+2,COL+2] ← 2.;
ROW ← ROW + 3;
COL ← COL + 4;
ALOC ← ALOC + 3*N + 4;
Q ← THREAD:NEXT[FIRST];
P ← THREAD:NEXT[Q];
FOR SEG ← 2 STEP 1 UNTIL NS DO
BEGIN "asegpol"
! Look at segment twixt Q and P;
IF THREAD:PLACE[Q] = RNULL
THEN BEGIN "auncnst" ! Left of segment is unconstrained;
MEM(ALOC) ← 1.;
MEM(ALOC-4) ← MEM(ALOC-3) ← MEM(ALOC-2) ← MEM(ALOC-1) ←
-1.;
ALOC ← ALOC + N;
MEM(ALOC-3) ← -1.;
MEM(ALOC-2) ← -2.;
MEM(ALOC-1) ← -3.;
MEM(ALOC+1) ← TEMP ← THREAD:STIME[Q]/THREAD:STIME[P];
ALOC ← ALOC + N;
MEM(ALOC-2) ← -1.;
MEM(ALOC-1) ← -3.;
MEM(ALOC+2) ← TEMP*TEMP;
! Same effect as:
A[ROW,COL] ← 1.
A[ROW,COL-1] ← A[ROW,COL-2] ← A[ROW,COL-3]
← A[ROW,COL-4] ← A[ROW+1,COL-3]
← A[ROW+2,COL-2] ← -1.
A[ROW+1,COL-1] ← A[ROW+2,COL-1] ← -3.
A[ROW+1,COL-2] ← -2.
A[ROW+2,COL+2] ← (A[ROW+1,COL+1] ←
THREAD:STIME[Q]/THREAD:STIME[P]) ↑ 2;
ROW ← ROW + 3;
COL ← COL + 4;
ALOC ← ALOC + N + 4;
END "auncnst"
ELSE BEGIN "acnst" ! Left of segment is constrained;
MEM(ALOC-1) ← MEM(ALOC-2) ← MEM(ALOC-3) ← MEM(ALOC-4)
← MEM(ALOC+N) ← 1.;
ALOC ← ALOC + N + N;
MEM(ALOC-1) ← -3.;
MEM(ALOC-2) ← -2.;
MEM(ALOC-3) ← -1.;
MEM(ALOC+1) ← TEMP ← THREAD:STIME[Q] / THREAD:STIME[P];
ALOC ← ALOC + N;
MEM(ALOC-2) ← -1.;
MEM(ALOC-1) ← -3.;
MEM(ALOC+2) ← TEMP*TEMP;
! Equivalent to:
A[ROW,COL-1] ← A[ROW,COL-2]
← A[ROW,COL-3] ← A[ROW,COL-4] ← A[ROW+1,COL] ← 1.
A[ROW+2,COL-3] ← A[ROW+3,COL-2] ← -1.
A[ROW+2,COL-1] ← A[ROW+3,COL-1] ← -3.
A[ROW+2,COL-2] ← -2.
A[ROW+3,COL+2] ← (A[ROW+2,COL+1] ←
THREAD:STIME[Q]/THREAD:STIME[P]) ↑ 2;
ROW ← ROW + 4;
COL ← COL + 4;
ALOC ← ALOC + N + 4;
END "acnst";
Q ← P;
P ← THREAD:NEXT[Q];
END "asegpol";
! Take care of the constraints at the final point;
COL ← COL - 4;
MEM(ALOC-4) ← MEM(ALOC-3) ← MEM(ALOC-2) ← MEM(ALOC-1) ← 1.;
ALOC ← ALOC + N;
MEM(ALOC-3) ← 1.;
MEM(ALOC-2) ← 2.;
MEM(ALOC-1) ← 3.;
ALOC ← ALOC + N;
MEM(ALOC-2) ← 2.;
MEM(ALOC-1) ← 6.;
! Equivalent to:
A[ROW,COL] ← A[ROW,COL+1] ← A[ROW,COL+2] ← A[ROW,COL+3]
← A[ROW+1,COL+1] ← 1.
A[ROW+1,COL+2] ← A[ROW+2,COL+2] ← 2.
A[ROW+1,COL+3] ← 3.
A[ROW+2,COL+3] ← 6.;
ROW ← ROW + 3;
COL ← COL + 4;
IF ROW ≠ COL ∨ ROW ≠ N + 1 THEN COMERR("ERROR IN POLY");
IF DEBUG ! Debug is defined false. Use RAID to remove the
jump around this code if you want to see the matrices;
THEN BEGIN "adebug" ! Print out the matrix A;
INTEGER WIDTH, DIGITS;
OUTSTR(CRLF);
GETFORMAT(WIDTH,DIGITS);
SETFORMAT(5,2);
FOR ROW ← 1 STEP 1 UNTIL N DO
BEGIN
FOR COL ← 1 STEP 1 UNTIL N DO
OUTSTR(CVF(A[ROW,COL]));
OUTSTR(CRLF);
END;
OUTSTR(CRLF);
SETFORMAT(WIDTH,DIGITS);
END "adebug";
DECOMPOSE(N,A,A);
! POLY continued: The B vectors;
! For each joint, calculate B, solve X and stow away;
FOR JOINT ← LOJ STEP 1 UNTIL HIJ DO
BEGIN "bcalc"
ARRCLR(B);
ROW ← 1;
! Fill the B matrix for the first segment;
B[ROW] ← THREAD:ANGLES[FIRST][JOINT];
B[ROW+1] ← THREAD:STIME[FIRST] * THREAD:VELS[FIRST][JOINT];
! If we ever put in non-zero acceleration constraints:
B[ROW+2] ← THREAD:STIME[FIRST][JOINT]↑2 * THREAD:ACCS[FIRST][JOINT];
ROW ← ROW + 3;
Q ← THREAD:NEXT[FIRST];
P ← THREAD:NEXT[Q];
FOR SEG ← 2 STEP 1 UNTIL NS DO
BEGIN "bsegpol"
! Look at segment twixt Q and P;
IF THREAD:PLACE[Q] = RNULL
THEN BEGIN "buncnst" ! Left of segment is unconstrained;
ROW ← ROW + 3;
END "buncnst"
ELSE BEGIN "bcnst" ! Left of segment is constrained;
B[ROW] ← B[ROW+1] ← THREAD:ANGLES[Q][JOINT];
ROW ← ROW + 4;
END "bcnst";
Q ← P;
P ← THREAD:NEXT[Q];
END "bsegpol";
! Take care of the constraints at the final point;
B[ROW] ← THREAD:ANGLES[LAST][JOINT];
B[ROW+1] ← THREAD:STIME[LAST] * THREAD:VELS[LAST][JOINT];
! If we ever put in non-zero acceleration constraints:
B[ROW+2] ← THREAD:STIME[LAST]↑2 * THREAD:ACCS[LAST][JOINT];
ROW ← ROW + 3;
IF ROW ≠ N + 1 THEN COMERR("ERROR IN POLY");
IF DEBUG ! Debug is defined false. Use RAID to remove the
jump around this code if you want to see the matrices;
THEN BEGIN "bdebug" ! Print out the matrix B;
INTEGER WIDTH, DIGITS;
OUTSTR(CRLF);
GETFORMAT(WIDTH,DIGITS);
SETFORMAT(5,2);
OUTSTR(CRLF);
FOR ROW ← 1 STEP 1 UNTIL N DO
OUTSTR(CVF(B[ROW]));
OUTSTR(CRLF);
SETFORMAT(WIDTH,DIGITS);
END "bdebug";
SOLVE(N,A,B,X);
! Stow away the answer into the coefficient matrices;
P ← THREAD:NEXT[FIRST];
I ← 1;
FOR SEG ← 1 STEP 1 UNTIL NS DO
BEGIN "stow" ! Each iteration stores the coefficients into one node;
ARRBLT(THREAD:COEFF[P][JOINT,0],X[I],4);
I ← I + 4;
P ← THREAD:NEXT[P];
END "stow";
END "bcalc";
END "poly";
! Main body of TRJCLC starts here;
RPTR(THREAD)
MOTION, ! The entire motion will be stored on this thread;
LEADTHREAD, ! A forward pointer used in scanning down thread;
CURTHREAD, OLDTHREAD; ! Used to trace down the motion;
REAL ARRAY DEL [1:14]; ! Joint angle differences;
RANY P, Q, ! Used in tracking down cell links;
TMPPLACE; ! Used as a temp in location calculations;
RPTR(VARIABLE) VAR;
REAL UT, ST, TT; ! User-defined time, system-computed time, total time;
INTEGER M, ! Holds modes;
FLAG, ! Boolean, for success parameters;
JOINT, ! For loop control;
I, ! For loop control;
LOJOINT, HIJOINT, ! Defines which arm;
ARM, ! Mechanism bits for the device used, eg YARM_MECH;
SBITS, ! Status bits for the device used, eg YARMSB;
SEGCNT, ! How many segments to the motion;
LAB, ! For code emission: a label;
DPTR, SEGLEN; ! For code emission: pointers into DATA and RELOC;
INTEGER ARRAY DATA, RELOC [0:1000]; ! Used for emitting code;
DEVBITS(ARM,SBITS,LOJOINT,HIJOINT,MOVE$:CF[MOV]);
SEGCNT ← 0;
TT ← 0.;
! Initialize the first node of the motion;
MOTION ← NEW_RECORD(THREAD);
THREAD:PLACE[MOTION] ← MOVE$:CF[MOV]; ! active arm's current frame;
THREAD:MODE[MOTION] ← ENDP_MODE; ! endpoint;
MEMLOC(THREAD:ANGLES[MOTION],INTEGER) ← NEWARY(LOJOINT,HIJOINT,1);
MEMLOC(THREAD:VELS[MOTION],INTEGER) ← NEWARY(LOJOINT,HIJOINT,1);
TMPPLACE ← PPLANVAL(THREAD:PLACE[MOTION],WORLD,FLAG);
IF ¬FLAG THEN COMERR("Illegal start point",MOV);
PLACESOL(THREAD:ANGLES[MOTION],TMPPLACE,ARM,FLAG);
IF ¬FLAG THEN
COMERR("The initial location is not accessible.
The closest reasonable point is being used.",MOV);
CURTHREAD ← MOTION;
! Treat the departure;
P ← MOVE$:CLAUSES[MOV];
WHILE (P≠RNULL) ∧ (RECTYPE(CELL:CAR[P])≠LOC(DEPROACH)) DO
P ← CELL:CDR[P];
IF P ≠ RNULL ∧ DEPROACH:DEPARTURE[CELL:CAR[P]]=TRUE
THEN BEGIN "depart" ! Won't work for fingers, of course;
RPTR(DEPROACH) DEPR;
IF LOJOINT = HIJOINT THEN COMERR("No deproaches allowed for fingers");
SEGCNT ← SEGCNT + 1;
OLDTHREAD ← CURTHREAD;
CURTHREAD ← THREAD:NEXT[OLDTHREAD] ← NEW_RECORD(THREAD);
THREAD:PLACE[CURTHREAD] ← DEPROACH:FRAME[CELL:CAR[P]];
MEMLOC(THREAD:ANGLES[CURTHREAD],INTEGER) ← NEWARY(LOJOINT,HIJOINT,1);
TMPPLACE ← PPLANVAL(THREAD:PLACE[CURTHREAD],WORLD,FLAG);
IF ¬FLAG THEN COMERR("Illegal departure point",MOV);
PLACESOL(THREAD:ANGLES[CURTHREAD],TMPPLACE,ARM,FLAG);
IF ¬FLAG THEN
COMERR("This departure location is not accessible.
The closest reasonable point is being used.",THREAD:PLACE[CURTHREAD]);
THREAD:STIME[CURTHREAD] ←
DEPTIME(THREAD:ANGLES[OLDTHREAD],THREAD:ANGLES[CURTHREAD],DEL);
TT ← TT + THREAD:STIME[CURTHREAD];
MEMLOC(THREAD:VELS[CURTHREAD],INTEGER) ← NEWARY(LOJOINT,HIJOINT,1);
MEMLOC(THREAD:COEFF[CURTHREAD],INTEGER) ← NEWARY(LOJOINT,HIJOINT,2);
FOR JOINT ← LOJOINT STEP 1 UNTIL HIJOINT DO
BEGIN ! stow away the coefficients for this joint.
The poly for each angle is -del*t↑4 + 2*del*t↑3 + initial;
REAL DL;
DL ← DEL[JOINT];
THREAD:COEFF[CURTHREAD][JOINT,4] ← -DL;
THREAD:COEFF[CURTHREAD][JOINT,3] ← 2. * DL;
THREAD:COEFF[CURTHREAD][JOINT,0] ←
THREAD:ANGLES[OLDTHREAD][JOINT];
THREAD:VELS[CURTHREAD][JOINT] ←
2. * DL / THREAD:STIME[CURTHREAD];
END;
THREAD:MODE[CURTHREAD] ← DEPA_MODE + INVI_MODE;
END "depart";
! Put intermediate points into the thread;
P ← MOVE$:CLAUSES[MOV];
WHILE TRUE DO
BEGIN "interm"
! This loop is terminated by a DONE. Each iteration looks at
the next via on the clauses list;
RPTR(VIA) VIAP;
WHILE (P≠RNULL) ∧ (RECTYPE(CELL:CAR[P])≠LOC(VIA)) DO
P ← CELL:CDR[P];
IF P = RNULL THEN DONE "interm";
VIAP ← CELL:CAR[P];
SEGCNT ← SEGCNT + 1;
OLDTHREAD ← CURTHREAD;
CURTHREAD ← THREAD:NEXT[OLDTHREAD] ← NEW_RECORD(THREAD);
THREAD:PLACE[CURTHREAD] ← VIA:ACTPLACE[VIAP];
MEMLOC(THREAD:ANGLES[CURTHREAD],INTEGER) ← NEWARY(LOJOINT,HIJOINT,1);
MEMLOC(THREAD:COEFF[CURTHREAD],INTEGER) ← NEWARY(LOJOINT,HIJOINT,2);
TMPPLACE ← PPLANVAL(THREAD:PLACE[CURTHREAD],WORLD,FLAG);
IF ¬FLAG THEN COMERR("Illegal via point",MOV);
PLACESOL(THREAD:ANGLES[CURTHREAD],TMPPLACE,ARM,FLAG);
IF ¬FLAG THEN
COMERR("This via location is not accessible.
The closest reasonable point is being used.",
CONS(MOV,CONS(THREAD:PLACE[CURTHREAD],RNULL)));
IF VIA:VELOC[VIAP] ≠ RNULL
THEN BEGIN ! There is a velocity specification here;
IF LOJOINT = HIJOINT
THEN THREAD:VELS[CURTHREAD][LOJOINT] ← SVAL:VAL[VIA:VELOC[VIAP]]
ELSE BEGIN "fvel"
RPTR(V3ECT) VTEMP; ! To hold offset vector (inches/second);
RPTR(FRAME) FTEMP; ! To hold frame value;
REAL ARRAY OFFANG [LOJOINT:HIJOINT]; ! Offset angles;
INTEGER I, ! Loop control;
FRLOC, OFLOC; ! Memory locations;
MEMLOC(THREAD:VELS[CURTHREAD],INTEGER) ← NEWARY(LOJOINT,HIJOINT,1);
FTEMP ← PPLANVAL(THREAD:PLACE[CURTHREAD],WORLD);
VTEMP ← VPLANVAL(VIA:VELOC[VIAP],WORLD);
FRLOC ← LOCATION(V3ECT:X[TRANS:P[FRAME:VAL[FTEMP]]]);
OFLOC ← LOCATION(V3ECT:X[VTEMP]);
FOR I ← 0 STEP 1 UNTIL 2 DO ! Add in offset vector;
MEM[OFLOC+I,REAL] ← MEM[OFLOC+I,REAL]/5. + MEM[FRLOC+I,REAL];
ARRBLT(OFFANG[LOJOINT],TINFO:ANGLES[FRAME:TINFO[FTEMP]][LOJOINT],6);
! So the out-of bounds result will be reasonable;
ARMSOL(OFFANG[LOJOINT],IF LOJOINT=1 THEN 0 ELSE 1,
ROTN:RMX[TRANS:R[FRAME:VAL[FTEMP]]], V3ECT:X[VTEMP]);
FOR JOINT ← LOJOINT STEP 1 UNTIL LOJOINT+5 DO
THREAD:VELS[CURTHREAD][JOINT] ←
5.*(OFFANG[JOINT] - THREAD:ANGLES[CURTHREAD][JOINT]);
END "fvel";
END;
ST ← THREAD:STIME[CURTHREAD] ←
RUNTIME(THREAD:ANGLES[OLDTHREAD],THREAD:ANGLES[CURTHREAD]);
IF VIA:TIME[VIAP] ≠ RNULL
THEN BEGIN ! The time is constrained;
RPTR(DURATION) DUR;
DUR ← VIA:TIME[VIAP];
UT ← THREAD:UTIME[CURTHREAD]
← SVAL:VAL[SPLANVAL(DURATION:TIME[DUR],WORLD)];
M ← THREAD:MODE[CURTHREAD] ← DURATION:TIME_RELN[DUR];
! test for incompatibilites;
IF ST > UT ∧ M ≥ 2 THEN
BEGIN
COMERR(
"Cannot satisfy your time request for this segment without danger;
you want "&CVG(UT)&" seconds, and I think you need "&CVG(ST)&"
seconds. Nonetheless, I am using your request.");
IF UT ≤ 0 THEN
BEGIN
COMERR("But I refuse to let you get away with no time at all!");
UT ← ST;
END;
END;
IF (M=1 ∧ ST<UT) ∨ (M=2 ∧ ST>UT) ∨ (M=3 ∧ ST≠UT) THEN
THREAD:STIME[CURTHREAD] ← UT;
END;
TT ← TT + THREAD:STIME[CURTHREAD];
P ← CELL:CDR[P];
END "interm";
! Treat the approach;
P ← MOVE$:CLAUSES[MOV];
WHILE (P≠RNULL) ∧
(RECTYPE(CELL:CAR[P])≠LOC(DEPROACH) ∨
DEPROACH:DEPARTURE[CELL:CAR[P]]=TRUE) DO
P ← CELL:CDR[P];
IF P ≠ RNULL
THEN BEGIN "approa" ! Will not work for finger operation;
IF LOJOINT = HIJOINT THEN COMERR("No deproaches allowed for fingers");
SEGCNT ← SEGCNT + 1;
OLDTHREAD ← CURTHREAD;
CURTHREAD ← THREAD:NEXT[OLDTHREAD] ← NEW_RECORD(THREAD);
THREAD:PLACE[CURTHREAD] ← DEPROACH:FRAME[CELL:CAR[P]];
MEMLOC(THREAD:ANGLES[CURTHREAD],INTEGER) ← NEWARY(LOJOINT,HIJOINT,1);
TMPPLACE ← PPLANVAL(THREAD:PLACE[CURTHREAD],WORLD,FLAG);
IF ¬FLAG THEN COMERR("Illegal approach point",MOV);
PLACESOL(THREAD:ANGLES[CURTHREAD],TMPPLACE,ARM,FLAG);
IF ¬FLAG THEN
COMERR("This approach location is not accessible.
The closest reasonable point is being used.",THREAD:PLACE[CURTHREAD]);
THREAD:STIME[CURTHREAD] ←
RUNTIME(THREAD:ANGLES[OLDTHREAD],THREAD:ANGLES[CURTHREAD]);
TT ← TT + THREAD:STIME[CURTHREAD];
MEMLOC(THREAD:VELS[CURTHREAD],INTEGER) ← NEWARY(LOJOINT,HIJOINT,1);
MEMLOC(THREAD:COEFF[CURTHREAD],INTEGER) ← NEWARY(LOJOINT,HIJOINT,2);
THREAD:MODE[CURTHREAD] ← APPR_MODE;
SEGCNT ← SEGCNT + 1;
OLDTHREAD ← CURTHREAD;
CURTHREAD ← THREAD:NEXT[OLDTHREAD] ← NEW_RECORD(THREAD);
THREAD:PLACE[CURTHREAD] ← MOVE$:DEXP[MOV];
THREAD:MODE[CURTHREAD] ← ENDP_MODE + INVI_MODE;
MEMLOC(THREAD:ANGLES[CURTHREAD],INTEGER) ← NEWARY(LOJOINT,HIJOINT,1);
TMPPLACE ← PPLANVAL(THREAD:PLACE[CURTHREAD],WORLD,FLAG);
IF ¬FLAG THEN COMERR("Illegal destination point",MOV);
PLACESOL(THREAD:ANGLES[CURTHREAD],TMPPLACE,ARM,FLAG);
IF ¬FLAG THEN
COMERR("This destination location is not accessible.
The closest reasonable point is being used.",
CONS(MOV,CONS(THREAD:PLACE[CURTHREAD],RNULL)));
THREAD:STIME[CURTHREAD] ←
DEPTIME(THREAD:ANGLES[CURTHREAD],THREAD:ANGLES[OLDTHREAD],DEL);
TT ← TT + THREAD:STIME[CURTHREAD];
MEMLOC(THREAD:VELS[CURTHREAD],INTEGER) ← NEWARY(LOJOINT,HIJOINT,1);
MEMLOC(THREAD:COEFF[CURTHREAD],INTEGER) ← NEWARY(LOJOINT,HIJOINT,2);
FOR JOINT ← LOJOINT STEP 1 UNTIL HIJOINT DO
BEGIN ! stow away the coefficients for this joint. If
del is (final - initial), then the poly for each angle is
{del*t↑4 - 2*del*t↑3 + 2*del*t + initial}, but note that
DEL holds the negative of del at this point;
REAL DL;
DL ← - DEL[JOINT];
THREAD:COEFF[CURTHREAD][JOINT,4] ← DL;
THREAD:COEFF[CURTHREAD][JOINT,3] ← -2. * DL;
THREAD:COEFF[CURTHREAD][JOINT,1] ← 2. * DL;
THREAD:COEFF[CURTHREAD][JOINT,0] ←
THREAD:ANGLES[OLDTHREAD][JOINT];
THREAD:VELS[OLDTHREAD][JOINT] ←
2. * DL / THREAD:STIME[CURTHREAD];
END;
END "approa"
ELSE BEGIN "arrive"
! There is no deproach point, just put in the final point;
SEGCNT ← SEGCNT + 1;
OLDTHREAD ← CURTHREAD;
CURTHREAD ← THREAD:NEXT[OLDTHREAD] ← NEW_RECORD(THREAD);
THREAD:PLACE[CURTHREAD] ← MOVE$:DEXP[MOV];
THREAD:MODE[CURTHREAD] ← ENDP_MODE;
MEMLOC(THREAD:ANGLES[CURTHREAD],INTEGER) ← NEWARY(LOJOINT,HIJOINT,1);
MEMLOC(THREAD:VELS[CURTHREAD],INTEGER) ← NEWARY(LOJOINT,HIJOINT,1);
TMPPLACE ← PPLANVAL(THREAD:PLACE[CURTHREAD],WORLD,FLAG);
IF ¬FLAG THEN COMERR("Illegal destination point",MOV);
PLACESOL(THREAD:ANGLES[CURTHREAD],TMPPLACE,ARM,FLAG);
IF ¬FLAG THEN
COMERR("This destination location is not accessible.
The closest reasonable point is being used.",
CONS(MOV,CONS(THREAD:PLACE[CURTHREAD],RNULL)));
THREAD:STIME[CURTHREAD] ←
RUNTIME(THREAD:ANGLES[OLDTHREAD],THREAD:ANGLES[CURTHREAD]);
TT ← TT + THREAD:STIME[CURTHREAD];
MEMLOC(THREAD:COEFF[CURTHREAD],INTEGER) ← NEWARY(LOJOINT,HIJOINT,2);
END "arrive";
! Check for overall time constraints. Fulfil them if possible;
P ← MOVE$:CLAUSES[MOV];
WHILE (P≠RNULL) ∧ (RECTYPE(CELL:CAR[P])≠LOC(DURATION)) DO
P ← CELL:CDR[P];
IF P ≠ RNULL
THEN BEGIN "timcst"
! There is a global time constraint. Must try to fulfil it;
REAL AVAIL, CURTIM, FACTOR;
INTEGER M;
UT ← SVAL:VAL[SPLANVAL(DURATION:TIME[CELL:CAR[P]],WORLD)];
IF UT>TT ∧ (DURATION:TIME_RELN[CELL:CAR[P]] LAND '1) ! (> or =);
THEN BEGIN "stretch" ! Easy case;
AVAIL ← 0.; ! Restricted extra time;
CURTIM ← 0; ! Currently used unrestricted time;
CURTHREAD ← THREAD:NEXT[MOTION]; ! First segment has no time;
WHILE CURTHREAD ≠ RNULL DO
BEGIN "timchk"
IF ¬((M←THREAD:MODE[CURTHREAD]) LAND INVI_MODE)
THEN ! Time in this segment not inviolate;
IF (M←M LAND '3) ≤ 1
THEN CURTIM ← CURTIM + THREAD:STIME[CURTHREAD]
ELSE IF M = 2
THEN AVAIL ← AVAIL + THREAD:UTIME[CURTHREAD]
- THREAD:STIME[CURTHREAD];
CURTHREAD ← THREAD:NEXT[CURTHREAD];
END "timchk";
IF CURTIM
THEN BEGIN "dostretch" ! Just modify those segments
which are not inviolate and have mode ≤ 1;
FACTOR ← (CURTIM + UT - TT) / CURTIM;
CURTHREAD ← THREAD:NEXT[MOTION];
WHILE CURTHREAD ≠ RNULL DO
BEGIN ! Expand right segments;
IF ¬((M←THREAD:MODE[CURTHREAD]) LAND INVI_MODE)
∧ (M LAND '3) ≤ 1
THEN THREAD:STIME[CURTHREAD]
← FACTOR * THREAD:STIME[CURTHREAD];
CURTHREAD ← THREAD:NEXT[CURTHREAD];
END;
END
ELSE COMERR(
"You want"&CVG(UT)&"seconds for this motion, and I could give you up
to"& CVG(TT+AVAIL) &", but I am only giving you"& CVG(TT)
&"instead.");
END "stretch"
ELSE IF UT<TT ∧ (DURATION:TIME_RELN[CELL:CAR[P]] LAND '3) ≥ 2 ! (< or =);
THEN BEGIN "shrink" ! Tough case;
COMERR(
"You want only" & CVG(UT) & "for this motion, and I think you need
" & CVG(TT) &". In order to satisfy your request, I am disregarding any
other time constraints you may have placed on the motion.");
CURTIM ← 0; ! Currently used non-inviolate time;
CURTHREAD ← THREAD:NEXT[MOTION]; ! First segment has no time;
WHILE CURTHREAD ≠ RNULL DO
BEGIN "timcnt"
IF ¬((M←THREAD:MODE[CURTHREAD]) LAND INVI_MODE)
THEN ! Time in this segment not inviolate;
CURTIM ← CURTIM + THREAD:STIME[CURTHREAD];
CURTHREAD ← THREAD:NEXT[CURTHREAD];
END "timcnt";
FACTOR ← (CURTIM + UT - TT) / CURTIM;
IF FACTOR ≤ 0
THEN BEGIN
COMERR(
"Your overall time constraint of" & CVG(UT) & "is ridiculous; I am
ignoring it.");
FACTOR ← 1.;
END;
CURTHREAD ← THREAD:NEXT[MOTION];
WHILE CURTHREAD ≠ RNULL DO
BEGIN ! Contract right segments;
IF ¬((M←THREAD:MODE[CURTHREAD]) LAND INVI_MODE)
THEN THREAD:STIME[CURTHREAD]
← FACTOR * THREAD:STIME[CURTHREAD];
CURTHREAD ← THREAD:NEXT[CURTHREAD];
END;
END "shrink";
END "timcst";
! Call the polynomial generator on chunks of the motion.
A chunk contains all segments between two velocity-constrained points;
OLDTHREAD ← MOTION;
CURTHREAD ← THREAD:NEXT[OLDTHREAD];
WHILE CURTHREAD ≠ RNULL DO
BEGIN "chunk"
! Each iteration finds one chunk, brackets it between
OLDTHREAD and CURTHREAD, and makes polys;
INTEGER PNTCNT; ! Counts number of points in each chunk;
PNTCNT ← 2; ! Count the end nodes this way;
WHILE (MEMLOC(THREAD:VELS[CURTHREAD],INTEGER) = 0) DO
BEGIN ! This chunk includes node pointed to by CURTHREAD;
PNTCNT ← PNTCNT + 1;
CURTHREAD ← THREAD:NEXT[CURTHREAD];
END;
! Now OLDTHREAD and CURTHREAD point to nodes on each end of
chunk;
IF PNTCNT = 2
THEN IF THREAD:MODE[OLDTHREAD]=APPR_MODE ∨
THREAD:MODE[CURTHREAD]=DEPA_MODE
THEN ! This is an approach or departure segment, so
the polynomials have already been calculated;
ELSE
! Two-point system. Use the fifth-order polynomial
which will meet the position, velocity, (and
acceleration) constraints. This is faster than the
older version which inserted two equispaced
unconstrained points and solved third degree
polynomials using POLY;
FOR JOINT ← LOJOINT STEP 1 UNTIL HIJOINT DO
BEGIN "oneseg"
REAL P0, V0, A0, P1, V1, A1;
! Initial, final position, vel, acc;
OWN REAL ARRAY COE[0:5]; ! For efficiency;
ST ← THREAD:STIME[CURTHREAD];
P0 ← THREAD:ANGLES[OLDTHREAD][JOINT];
P1 ← THREAD:ANGLES[CURTHREAD][JOINT];
V0 ← THREAD:VELS[OLDTHREAD][JOINT]*ST;
V1 ← THREAD:VELS[CURTHREAD][JOINT]*ST;
A0 ← 0.0; ! *ST*ST;
A1 ← 0.0; ! *ST*ST;
COE[0] ← P0;
COE[1] ← V0;
COE[2] ← A0/2.;
COE[3] ← -(8.*V1 + 12.*V0 + 3.*A0 - A1 - 20.*P1 + 20.*P0) / 2.;
COE[4] ← (14.*V1 + 16.*V0 + 3.*A0 - 2.*A1 - 30.*P1 + 30.*P0) / 2.;
COE[5] ← -(6.*V1 + 6.*V0 + A0 - A1 - 12.*P1 + 12.*P0) / 2.;
ARRBLT(THREAD:COEFF[CURTHREAD][JOINT,0],COE[0],6);
END "oneseg"
ELSE BEGIN "sevseg" ! There are several segments. Find
the two with longest intervals, and put free
points there. There is some dispute as to the
best place to put these points. The current
version puts them very close to the beginning of
the interval. An older version put them in the
middle, and this led to massive overshoots;
REAL T1, T2; ! Longest, next longest times;
RPTR(THREAD) Q1, Q2; ! Longest, next longest segment starters;
T1 ← T2 ← 0.;
Q ← OLDTHREAD;
P ← THREAD:NEXT[Q];
WHILE Q ≠ CURTHREAD DO
BEGIN "max" ! This loop finds the two longest intervals;
IF (ST←THREAD:STIME[P]) > T2
THEN IF ST > T1
THEN BEGIN ! New longest;
T2 ← T1;
T1 ← ST;
Q2 ← Q1;
Q1 ← Q;
END
ELSE BEGIN ! New next-longest;
T2 ← ST;
Q2 ← Q;
END;
Q ← P;
P ← THREAD:NEXT[P]
END "max";
P ← THREAD:NEXT[Q1];
Q ← THREAD:NEXT[Q1] ← NEW_RECORD(THREAD);
THREAD:NEXT[Q] ← P;
THREAD:STIME[Q] ← T1 * 0.001;
THREAD:STIME[P] ← T1 - THREAD:STIME[Q];
MEMLOC(THREAD:COEFF[Q],INTEGER) ← NEWARY(LOJOINT,HIJOINT,2);
P ← THREAD:NEXT[Q2];
Q ← THREAD:NEXT[Q2] ← NEW_RECORD(THREAD);
THREAD:NEXT[Q] ← P;
THREAD:STIME[Q] ← T2 * 0.001;
THREAD:STIME[P] ← T2 - THREAD:STIME[Q];
MEMLOC(THREAD:COEFF[Q],INTEGER) ← NEWARY(LOJOINT,HIJOINT,2);
POLY(OLDTHREAD,CURTHREAD,LOJOINT,HIJOINT,PNTCNT+1);
! FOR JOINT ← LOJOINT STEP 1 UNTIL HIJOINT DO
POLY(OLDTHREAD,CURTHREAD,JOINT,PNTCNT+1);
END "sevseg";
OLDTHREAD ← CURTHREAD;
CURTHREAD ← THREAD:NEXT[OLDTHREAD];
END "chunk";
! Compute the gravity and inertia terms;
CURTHREAD ← THREAD:NEXT[MOTION];
WHILE CURTHREAD ≠ RNULL DO
BEGIN "grav"
REQUIRE "BEJCZY.REL[11,BES]" LOAD_MODULE;
EXTERNAL PROCEDURE DTERMS(REFERENCE REAL RES, ANG; INTEGER ARM);
IF MEMLOC(THREAD:ANGLES[CURTHREAD],INTEGER) = 0
THEN BEGIN ! Need to set up angles array on this free point;
MEMLOC(THREAD:ANGLES[CURTHREAD],INTEGER) ← NEWARY(LOJOINT,HIJOINT,1);
FOR JOINT ← LOJOINT STEP 1 UNTIL HIJOINT DO
THREAD:ANGLES[CURTHREAD][JOINT] ← THREAD:COEFF[CURTHREAD][JOINT,0];
END;
MEMLOC(THREAD:GRAVIN[CURTHREAD],INTEGER) ← NEWARY(2*LOJOINT,2*HIJOINT+1,1);
DTERMS(THREAD:GRAVIN[CURTHREAD][2*LOJOINT],
THREAD:ANGLES[CURTHREAD][LOJOINT], ARM);
CURTHREAD ← THREAD:NEXT[CURTHREAD];
END "grav";
! Output the motion table;
! The format for the coefficients is described in ARM.PAL[11,BES].
SERVO BIT WORD
YARMSB, YHANDSB, BARMSB, or BHANDSB
SERVO BIT WORD
COMMAND BITS; DEFINE NONULLCB = "'1"; ! No end null;
DEFINE WOBBLECB = "'2"; ! Wobble at end;
DEFINE DEPARTCB = "'4"; ! ∃ Departure point
RELATIVE SEGMENT PTR Length (bytes) of first segment table
8 + 32*njoints. 0 means there are no more
segments. (Put at very end of table)
TIME milliseconds for this segment
TRANS ptr to list of transforms+valid. nos:
L-O Level-offset of first trans (or scalar)
VAL Place for validity number
MECH Mechanism number:
YARM_MECH, YHAND_MECH, BARM_MECH or BHAND_MECH
L-O Level-offset for next variable
:
MECH Last mechanism number
CODE ptr to code to be scheduled at end
of this segment
A0 coeff (floating) first joint
:
A5 last coeff, first joint
A0 first coeff, second jolint
:
:
:
A5 last coeff, last joint
NCI final joint gravity loading, first joint
NCII final joint inertia loading, first joint
:
:
NCI final joint gravity loading, last joint
NCII final joint inertia loading, last joint
RELATIVE SEGMENT PTR
This is the format for the transform-validity list:
T1 level-offset of transform for first arm
0 room for validity number
:
Tn level-offset of transform for last arm
0 room for validity number
DATA and RELOC are used to output the motion table. Each of
these is 1000 long (very long motions may not fit.). See
EMITER.HDR for the pseudo-op definitions.
;
ARRCLR(RELOC);
ARRCLR(DATA);
! Output pseudo-op for motion;
LAB ← GENLABEL; ! Points to motion table;
EMIT(PSDCODE,MOVE_PSOP,PSINST);
EMIT(PSDCODE,LAB,SYMREF);
EMIT(PSDCODE,ARM,CONST);
! Output trajectory file;
MAKE_REMARK(TJFILE,"Motion table");
EMIT(TJFILE,LAB,SYMDEC);
DATA[4] ← SBITS; ! servo bits;
RELOC[4] ← CONST;
DATA[5] ← 0; ! No second servo bits;
DATA[6] ← ! want DEPARTCB only if this motion has a deproach;
NONULLCB LOR
(IF THREAD:MODE[THREAD:NEXT[MOTION]] LAND DEPA_MODE THEN DEPARTCB ELSE 0);
RELOC[5] ← RELOC[6] ← CONST;
EMIT(TJFILE,DATA[4],RELOC[4],3); ! Label, servo bits, servo bits, command bits;
SEGLEN ← 8 + 32*(IF (ARM = YARM_MECH) OR (ARM = BARM_MECH) THEN 6 ELSE 1);
DATA[7] ← SEGLEN; RELOC[7] ← CONST;
DEFINE OLDVERSION = "FALSE";
IFC OLDVERSION
THENC
P ← THREAD:NEXT[MOTION];
WHILE P ≠ RNULL DO
BEGIN "coeout" ! Coefficients for one segment;
MAKE_REMARK(TJFILE,"Relative segment pointer");
IF THREAD:NEXT[P]=RNULL THEN DATA[7] ← 0;
! The last relative segment pointer is supposed to be 0;
DATA[8] ← THREAD:STIME[P]*1000.; RELOC[8] ← CONST; ! Milliseconds;
EMIT(TJFILE,DATA[7],RELOC[7],2); ! Relative seg ptr, time;
IF THREAD:PLACE[P]≠RNULL ∧ RECTYPE(THREAD:PLACE[P])=LOC(VARIABLE)
THEN BEGIN "needtrans"
! There is an associated place, need a trans pointer;
EMIT(TJFILE,LAB←GENLABEL,SYMREF,1); ! Refer to the trans pointer;
MAKE_REMARK(SMLBLK,"Trans pointer for motion");
EMIT(SMLBLK,LAB,SYMDEC,1); ! Here is the trans pointer;
EMIT(SMLBLK,VARIABLE:OFFSET[THREAD:PLACE[P]],CONST,1);
! Point to the trans itself;
! Currently only one frame allowed;
EMIT(SMLBLK,0,CONST,1); ! Leave room for the validity bit;
END "needtrans"
ELSE EMIT(TJFILE,0,CONST,1); ! The absence of a trans pointer;
IF THREAD:EVENT[P]≠RNULL
THEN ! There is some associated code, need to point to it;
EMIT(TJFILE,STMNT:STLAB[THREAD:EVENT[P]],SYMREF,1)
ELSE EMIT(TJFILE,0,CONST,1); ! The absence of associated code;
DPTR ← 9;
MAKE_REMARK(TJFILE,"Coefficients, gravity, inertia");
! Coefficients;
FOR JOINT← LOJOINT STEP 1 UNTIL HIJOINT DO
BEGIN ! Each iteration spits out the coefficient of one joint;
INTEGER DEG;
FOR DEG ← 0 STEP 1 UNTIL 5 DO
INT_TO_11FLOAT(DATA[DPTR+2*DEG],DATA[DPTR+2*DEG+1],
THREAD:COEFF[P][JOINT,DEG]);
DPTR ← DPTR+12;
END;
! The gravity and inertia terms;
FOR I ← 2*LOJOINT STEP 1 UNTIL 2*HIJOINT+1 DO
BEGIN ! Each 2 iterations spits out the terms for one joint;
INT_TO_11FLOAT(DATA[DPTR],DATA[DPTR+1],
THREAD:GRAVIN[P][I]);
DPTR ← DPTR+2;
END;
RELOC[9] ← CONST; ! They are all constants;
ARRBLT(RELOC[10],RELOC[9],DPTR-10);
EMIT(TJFILE,DATA[9],RELOC[9],DPTR-9); ! All the coefficients for this seg;
P ← THREAD:NEXT[P];
END "coeout";
MAKE_REMARK(TJFILE,"End of motion table");
ELSEC
Q ← THREAD:NEXT[MOTION];
WHILE Q ≠ RNULL DO
BEGIN "coeout" ! Coefficients for one segment;
P ← Q;
Q ← THREAD:NEXT[Q];
IF THREAD:PLACE[P] = RNULL THEN CONTINUE "coeout";
! Avoid outputting the short segments that end at
unconstrained points;
MAKE_REMARK(TJFILE,"Relative segment pointer");
DATA[8] ← THREAD:STIME[P]*1000.; RELOC[8] ← CONST; ! Milliseconds;
EMIT(TJFILE,DATA[7],RELOC[7],2); ! Relative seg ptr, time;
IF RECTYPE(THREAD:PLACE[P])=LOC(VARIABLE)
THEN BEGIN "needtrans"
! There is an associated place, need a trans pointer;
EMIT(TJFILE,LAB←GENLABEL,SYMREF,1); ! Refer to the trans pointer;
MAKE_REMARK(SMLBLK,"Trans pointer for motion");
EMIT(SMLBLK,LAB,SYMDEC,1); ! Here is the trans pointer;
EMIT(SMLBLK,VARIABLE:OFFSET[THREAD:PLACE[P]],CONST,1);
! Point to the trans itself;
! Currently only one frame allowed;
EMIT(SMLBLK,0,CONST,1); ! Leave room for the validity bit;
END "needtrans"
ELSE EMIT(TJFILE,0,CONST,1); ! The absence of a trans pointer;
IF THREAD:EVENT[P]≠RNULL
THEN ! There is some associated code, need to point to it;
EMIT(TJFILE,STMNT:STLAB[THREAD:EVENT[P]],SYMREF,1)
ELSE EMIT(TJFILE,0,CONST,1); ! The absence of associated code;
DPTR ← 9;
MAKE_REMARK(TJFILE,"Coefficients, gravity, inertia");
! Coefficients;
FOR JOINT← LOJOINT STEP 1 UNTIL HIJOINT DO
BEGIN ! Each iteration spits out the coefficient of one joint;
INTEGER DEG;
FOR DEG ← 0 STEP 1 UNTIL 5 DO
INT_TO_11FLOAT(DATA[DPTR+2*DEG],DATA[DPTR+2*DEG+1],
THREAD:COEFF[P][JOINT,DEG]);
DPTR ← DPTR+12;
END;
! The gravity and inertia terms;
FOR I ← 2*LOJOINT STEP 1 UNTIL 2*HIJOINT+1 DO
BEGIN ! Each 2 iterations spits out the terms for one joint;
INT_TO_11FLOAT(DATA[DPTR],DATA[DPTR+1],
THREAD:GRAVIN[P][I]);
DPTR ← DPTR+2;
END;
RELOC[9] ← CONST; ! They are all constants;
ARRBLT(RELOC[10],RELOC[9],DPTR-10);
EMIT(TJFILE,DATA[9],RELOC[9],DPTR-9); ! All the coefficients for this seg;
END "coeout";
EMIT(TJFILE,0,CONST,1); ! The last relative segment pointer is 0;
MAKE_REMARK(TJFILE,"End of motion table");
ENDC
! Reclaim all the arrays in the motion thread;
Q ← MOTION;
WHILE Q ≠ RNULL DO
BEGIN "reclaim"
INTEGER ADR;
EXTERNAL PROCEDURE ARYEL (INTEGER ADRESS); ! In the SAIL segment;
ADR ← 0;
MEMLOC(THREAD:ANGLES[Q],INTEGER) ↔ ADR;
IF ADR THEN ARYEL(ADR);
ADR ← 0;
MEMLOC(THREAD:VELS[Q],INTEGER) ↔ ADR;
IF ADR THEN ARYEL(ADR);
ADR ← 0;
MEMLOC(THREAD:COEFF[Q],INTEGER) ↔ ADR;
IF ADR THEN ARYEL(ADR);
ADR ← 0;
MEMLOC(THREAD:GRAVIN[Q],INTEGER) ↔ ADR;
IF ADR THEN ARYEL(ADR);
Q ← THREAD:NEXT[Q];
END "reclaim";
! End of TRJCLC;
END "trjclc";
! CENTCLC, STOPCLC;
INTERNAL PROCEDURE CENTCLC (RPTR(CENTER) CNTR);
BEGIN "centclc"
! The "trajectory" table looks like this:
COFLST: XXXXXX TWO SERVO BIT WORDS, 7 BITS MUST BE ON, A HAND
XXXXXX SERVO AND ALL JOINT SERVOS OF THE SAME ARM
0 NO COMMAND BITS
0 NO NEXT SEGMENT
0 NO FUNCTION TIME
0 NO TRANSFORM
CODE PTR TO CODE TO BE SCHEDULED THIS SEG
NO POLYNOMIAL TO FOLLOW
;
! Does not yet handle any cmons or code;
INTEGER ARM, SBITS, LAB;
INTEGER LOJOINT, HIJOINT; ! Not used;
PRELOAD_WITH 0,0,0,0,0,0;
OWN INTEGER ARRAY ZEROS[1:6];
PRELOAD_WITH CONST,CONST,CONST,CONST,CONST,CONST;
OWN INTEGER ARRAY CONSTS[1:6];
DEVBITS(ARM,SBITS,LOJOINT,HIJOINT,CENTER:CF[CNTR]);
! Want to turn on the hand bits as well:
IF ARM = YARM_MECH
THEN BEGIN
ARM ← YARM_MECH + YHAND_MECH
SBITS ← YARMSB + YHANDSB
END
ELSE IF ARM = BARM_MECH
THEN BEGIN
ARM ← BARM_MECH + BHAND_MECH
SBITS ← BARMSB + BHANDSB
END;
! Do it like this:;
ARM ← ARM LOR (ARM LSH 1);
SBITS ← SBITS LOR (SBITS LSH -1);
EMIT(PSDCODE,CENTER_PSOP,PSINST);
LAB ← GENLABEL;
EMIT(PSDCODE,LAB,SYMREF);
EMIT(PSDCODE,ARM,CONST);
MAKE_REMARK(TJFILE,"Center table");
EMIT(TJFILE,LAB,SYMDEC);
EMIT(TJFILE,SBITS,CONST);
EMIT(TJFILE,ZEROS[1],CONSTS[1],6);
END "centclc";
INTERNAL PROCEDURE STOPCLC(RPTR(STOP) STP);
BEGIN "stopclc"
INTEGER ARM, SBITS, LOJOINT, HIJOINT; ! Only ARM is used;
DEVBITS(ARM,SBITS,LOJOINT, HIJOINT, STOP:CF[STP]);
EMIT(PSDCODE,STOP_PSOP,PSINST);
EMIT(PSDCODE,SBITS,CONST);
END "stopclc";
END $$prgid;
! Bugs
;